#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/string.hpp>
#include <nlohmann/json.hpp>
#include "webrtc_pub.hpp"

using nlohmann::json;

class ROSWebRTCNode : public rclcpp::Node {
   public:
    ROSWebRTCNode() : Node("webrtc_node"), webrtc_pub_("ROS_ROBOT_001"), use_compression_(true) {
        // 声明参数
        this->declare_parameter("use_draco_compression", true);
        this->declare_parameter("draco_compression_level", 7);
        
        // 获取参数值
        use_compression_ = this->get_parameter("use_draco_compression").as_bool();
        int compression_level = this->get_parameter("draco_compression_level").as_int();
        
        RCLCPP_INFO(this->get_logger(), "点云压缩设置: %s", use_compression_ ? "启用" : "禁用");
        if (use_compression_) {
            RCLCPP_INFO(this->get_logger(), "Draco压缩级别: %d", compression_level);
            RCLCPP_INFO(this->get_logger(), "Draco可用性: %s", webrtc_pub_.is_draco_available() ? "是" : "否");
        }
        // 初始化WebRTC客户端
        if (!webrtc_pub_.initialize()) {
            RCLCPP_ERROR(this->get_logger(), "WebRTC客户端初始化失败");
            return;
        }

        // 设置回调函数
        webrtc_pub_.set_connection_callback(std::bind(&ROSWebRTCNode::on_connection_status_changed, this, std::placeholders::_1));

        webrtc_pub_.set_data_received_callback(std::bind(&ROSWebRTCNode::on_data_received, this, std::placeholders::_1));

        // 初始化ROS发布者和订阅者
        pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/vanjee_points722", 10, std::bind(&ROSWebRTCNode::pointcloud_callback, this, std::placeholders::_1));

        connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>("/webrtc/connection_status", 1);
        room_info_pub_ = this->create_publisher<std_msgs::msg::String>("/webrtc/room_info", 1);

        // 连接到WebRTC房间
        //TODO webrtc 数据通道初始化
        connect_to_webrtc();

        RCLCPP_INFO(this->get_logger(), "ROS WebRTC节点已启动");
    }

    ~ROSWebRTCNode() {
        webrtc_pub_.disconnect();
        RCLCPP_INFO(this->get_logger(), "ROS WebRTC节点已停止");
    }

   private:
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr room_info_pub_;

    WebRTCClient webrtc_pub_;
    std::string current_room_id_;
    bool use_compression_;  // 是否使用Draco压缩

    void connect_to_webrtc() {
        RCLCPP_INFO(this->get_logger(), "正在获取WebRTC房间ID...");
        std::string room_id = webrtc_pub_.get_room_id();

        if (room_id.empty()) {
            RCLCPP_ERROR(this->get_logger(), "获取房间ID失败");
            return;
        }

        current_room_id_ = room_id;
        RCLCPP_INFO(this->get_logger(), "获取到房间ID: %s", room_id.c_str());

        // 发布房间信息
        auto room_msg = std_msgs::msg::String();
        room_msg.data = room_id;
        room_info_pub_->publish(room_msg);

        // 连接到房间
        RCLCPP_INFO(this->get_logger(), "正在连接到WebRTC房间...");
        if (webrtc_pub_.connect_to_room(room_id)) {//TODO 连接房间
            RCLCPP_INFO(this->get_logger(), "成功连接到WebRTC房间");
        } else {
            RCLCPP_ERROR(this->get_logger(), "连接到WebRTC房间失败");
        }
    }

    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        if (!webrtc_pub_.is_data_channel_ready()) {
            static auto last_warn_time = this->get_clock()->now();
            auto current_time = this->get_clock()->now();
            if ((current_time - last_warn_time).seconds() >= 5.0) {
                RCLCPP_WARN(this->get_logger(), "WebRTC数据通道未就绪，跳过点云数据");
                last_warn_time = current_time;
            }
            return;
        }

        // 时间控制：确保两帧数据间隔大于500ms
        static auto last_publish_time = rclcpp::Time(0, 0, RCL_ROS_TIME);
        auto current_time = this->get_clock()->now();
        auto time_diff = (current_time - last_publish_time).seconds() * 1000.0; // 转换为毫秒
        
        if (time_diff < 300.0 && last_publish_time.nanoseconds() != 0) {
            // 跳过此帧，时间间隔不够
            static auto last_skip_log_time = rclcpp::Time(0, 0, RCL_ROS_TIME);
            if ((current_time - last_skip_log_time).seconds() >= 2.0) {
                RCLCPP_DEBUG(this->get_logger(), "跳过点云帧，时间间隔仅为 %.1fms (< 500ms)", time_diff);
                last_skip_log_time = current_time;
            }
            return;
        }
        
        // 更新最后发布时间
        last_publish_time = current_time;

        try {
            // 转换ROS PointCloud2到PCL
            pcl::PointCloud<pcl::PointXYZI> cloud;
            pcl::fromROSMsg(*msg, cloud);

            // 转换为float数组
            std::vector<float> points;
            points.reserve(cloud.points.size() * 4);
            std::cout<<"point size: "<<cloud.points.size()<<std::endl;
            for (const auto& point : cloud.points) {
                points.push_back(point.x);
                points.push_back(point.y);
                points.push_back(point.z);
                points.push_back(static_cast<float>(point.intensity));
            }

            static auto last_info_time = this->get_clock()->now();
            auto info_current_time = this->get_clock()->now();
            if ((info_current_time - last_info_time).seconds() >= 1.0) {
                size_t original_size = cloud.points.size() * 4 * sizeof(float);
                RCLCPP_INFO(this->get_logger(), "处理点云数据: %zu个点，原始大小: %.2f KB，压缩模式: %s", 
                           cloud.points.size(), original_size / 1024.0, 
                           (use_compression_ && webrtc_pub_.is_draco_available()) ? "Draco压缩" : "未压缩");
                last_info_time = info_current_time;
            }

            // 根据配置选择发送方式
            bool send_success = false;
            if (use_compression_ && webrtc_pub_.is_draco_available()) {
                // 使用Draco压缩发送
                send_success = webrtc_pub_.send_compressed_point_cloud(points);
                if (!send_success) {
                    RCLCPP_WARN(this->get_logger(), "Draco压缩发送失败，尝试未压缩方式");
                    send_success = webrtc_pub_.send_point_cloud_data(points);
                }
            } else {
                // 使用原始方式发送
                send_success = webrtc_pub_.send_point_cloud_data(points);
            }

            if (!send_success) {
                RCLCPP_WARN(this->get_logger(), "发送点云数据失败");
            }

        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "处理点云数据时出错: %s", e.what());
        }
    }

    void on_connection_status_changed(bool connected) {
        auto status_msg = std_msgs::msg::Bool();
        status_msg.data = connected;
        connection_status_pub_->publish(status_msg);

        if (connected) {
            RCLCPP_INFO(this->get_logger(), "WebRTC连接已建立");
        } else {
            RCLCPP_WARN(this->get_logger(), "WebRTC连接已断开");
            // 可以在这里实现重连逻辑
        }
    }

    void on_data_received(const std::vector<uint8_t>& data) {
        RCLCPP_INFO(this->get_logger(), "收到WebRTC数据: %zu bytes", data.size());
        // 可以在这里处理接收到的数据
    }
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    try {
        auto node = std::make_shared<ROSWebRTCNode>();
        rclcpp::spin(node);
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("webrtc_node"), "节点运行时出错: %s", e.what());
        return 1;
    }

    rclcpp::shutdown();
    return 0;
}